#include "lpc17xx_i2c.h"
#include "lpc17xx_pinsel.h"
#include "lpc_types.h"

#include "timer.h"
#include "SERIALAPI.h"
#include "i2c_msg_codes.h"

#define FRONT_SENS  (1 << 17)
#define DELAY_INCRMNT 10
#define I2C_SLAVE_ADDR 0x90

#define MSG_SIZE 0x02
#define QUEUE_SIZE 0x80
#define TRANSFER() I2C_SlaveTransferData(LPC_I2C2, &slave_data_cfg, I2C_TRANSFER_POLLING)
#define TIMER_T uint32_t

I2C_S_SETUP_Type slave_data_cfg;
uint32_t displacement_delay;

//Received messages are queued and executed periodically by displacement timer
uint8_t buffer[MSG_SIZE];
uint8_t msg_queue[QUEUE_SIZE*MSG_SIZE];
uint8_t *q_front, *q_back = msg_queue;

//Function Prototypes
void i2c_slave_setup(void);
void set_displacement_delay(uint8_t);
void inc_displacement_delay(void);
uint8_t* next_msg(uint8_t*);
uint8_t* prev_msg(uint8_t*);
void add_msg_to_queue(uint8_t*);
void exec_current_msg(void);

//** MAIN ENTRY POINT **//
int main(void){

  init_timer();

  //I2C CONFIG
  i2c_slave_setup();

  //Wait for master to start session
  TRANSFER();
  while(*buffer != I2C_MSG_OPN){/*waiting*/};

  //Calibrate displacement between bots
  set_displacement_delay(0x20);

  //Commence command timer using calibrated displacement delay
  TIMER_T command_timer;
  command_timer = enable_timer(1000, displacement_delay, exec_current_msg);
  
  //Wait for messages from master
  while (*buffer != I2C_MSG_CLS){

    if (TRANSFER() == ERROR){ //In case of broken link...

      if (*q_back != I2C_MSG_STP){ //Halt m3pi and wait for link
	buffer[0] = I2C_MSG_STP;
	add_msg_to_queue(buffer);
      }      
      continue;
    }

    //Arbitrate whether received message is queued or ignored
    switch (*buffer) {
      //MOTORS STOP
    case I2C_MSG_STP:
    case I2C_MSG_M1S:
    case I2C_MSG_M2S:
      //MOTORS DIRECTION
    case I2C_MSG_FWD:
    case I2C_MSG_REV:
    case I2C_MSG_M1F:
    case I2C_MSG_M1R:
    case I2C_MSG_M2F:
    case I2C_MSG_M2R:
      add_msg_to_queue(buffer);
      break;
      //SESSION
    case I2C_MSG_OPN:
    case I2C_MSG_CLS:
    default:
      break;      
    } 

  }

  //Teardown
  disable_timer(command_timer);
  I2C_DeInit(LPC_I2C2);

  return 0;
}


void i2c_slave_setup(void){

  PINSEL_CFG_Type PinCfg;  

  //Configure second I2C device (first in use by m3pi)
  PinCfg.Funcnum     = PINSEL_FUNC_2;
  PinCfg.OpenDrain   = PINSEL_PINMODE_NORMAL;
  PinCfg.Pinmode     = PINSEL_PINMODE_PULLUP;
  PinCfg.Portnum     = 0;
  PinCfg.Pinnum      = 27;
  PINSEL_ConfigPin(&PinCfg);
  PinCfg.Pinnum      = 28;
  PINSEL_ConfigPin(&PinCfg);

  I2C_Init(LPC_I2C2, 100000);
  
  //SET SLAVE ADDRESS
  I2C_OWNSLAVEADDR_CFG_Type slave_address_cfg;
  
  slave_address_cfg.GeneralCallState = ENABLE;
  slave_address_cfg.SlaveAddrChannel = 0;
  slave_address_cfg.SlaveAddrMaskValue = 0xFF;
  slave_address_cfg.SlaveAddr_7bit = I2C_SLAVE_ADDR;
  I2C_SetOwnSlaveAddr(LPC_I2C2, &slave_address_cfg);

  //SET SLAVE TO RECEIVE
  slave_data_cfg.tx_data = NULL;
  slave_data_cfg.tx_length = 0;
  slave_data_cfg.rx_data = buffer;
  slave_data_cfg.rx_length = sizeof(buffer);

  I2C_Cmd(LPC_I2C2, ENABLE);

}

void set_displacement_delay(uint8_t approach_speed){
  //Approach master robot, calibrate delay according to initial
  //displacement between both bots
  MOVE_FORWARD(approach_speed);

  //interrupt every 10ms by default
  TIMER_T displacement_timer = enable_timer(1000, DELAY_INCRMNT, inc_displacement_delay); 
  while(!(GPIO_ReadValue(0) & FRONT_SENS)){/*increment until other bot detected*/};
  disable_timer(displacement_timer);
  STOP_MOTORS();
}

void inc_displacement_delay(void){ //Increment delay on interrupt
  displacement_delay += DELAY_INCRMNT;
}

uint8_t* next_msg(uint8_t* msg){ //Make queue pointer incrementation safe
  uint8_t i, *j;

  for (i = 0; i < MSG_SIZE; i++){
    j = (msg == &msg_queue[QUEUE_SIZE-1]) ? &msg_queue[0] : j + 1;
  }

  return j;
}

uint8_t* prev_msg(uint8_t* msg){ //Make queue pointer decrementation safe
  uint8_t i, *j;

  for (i = 0; i < MSG_SIZE; i++){
    j = (msg == &msg_queue[0]) ? &msg_queue[QUEUE_SIZE-1] : j - 1;
  }

  return j;
}

void add_msg_to_queue(uint8_t* msg){ //copy recv'd message to back of queue

  if (next_msg(q_back) != q_front) 
    q_back = next_msg(q_back);
  else  
    return;

  uint8_t i;
  for (i = 0; i < MSG_SIZE; i++){//homebrewed strcopy
    *(q_back+i) = msg[i];
  }

}

void exec_current_msg(void){ //Execute msg at front of queue on interrupt

  //Return if queue is empty, effectively 'Suspends' displacement interrupt
  if (q_front == q_back) return; 

  //Pass message type and param to motor control
    switch (*q_front) {

      //MOTORS STOP
    case I2C_MSG_STP:
      STOP_MOTORS();      
      break;
    case I2C_MSG_M1S:
      M1_STOP();      
      break;
    case I2C_MSG_M2S:
      M2_STOP();      
      break;

      //MOTORS DIRECTION
    case I2C_MSG_FWD:
      MOVE_FORWARD(*(q_front+1));      
      break;
    case I2C_MSG_REV:
      MOVE_REVERSE(*(q_front+1));      
      break;
    case I2C_MSG_M1F:
      M1_FORWARD(*(q_front+1));      
      break;
    case I2C_MSG_M1R:
      M1_REVERSE(*(q_front+1));      
      break;
    case I2C_MSG_M2F:
      M2_FORWARD(*(q_front+1));      
      break;
    case I2C_MSG_M2R:
      M2_REVERSE(*(q_front+1));      
      break;
    default:
      break;      
    } 

    //Message consumed, move to next on queue
  q_front = next_msg(q_front);
}
